/*
 *  Vector.cpp
 *  T3nsors
 *
 *  Created by Michael Barriault on 10-09-23.
 *  Copyright 2010 MikBarr. All rights reserved.
 *
 */

#include <T3nsors/Vector.h>
#include <T3nsors/Macros.h>
#include <T3nsors/Others.h>
#include <cstdarg>
#include <cmath>
#include <cstdio>
using std::string;

T3::Vector::Vector(void) {
	return;
}

T3::Vector::Vector(int iN, std::string iid) {
	*this = T3::Vector(Domain(1,0,0.,0.),iN,iid);
}

T3::Vector::Vector(T3::Domain iO, int iN, std::string iid) {
	O = iO;
	N = iN;
	id = iid;
	T.assign(N, T3::Scalar(O,id));
	FOR(n,N)
		T[n].id = T[n].id + itos(n);
}

T3::Scalar& T3::Vector::operator()(int i) {
	return T[i];
}

real T3::Vector::rad(int o) {
	real r = 0;
	FOR(n,N) r += T[n][o]*T[n][o];
	return sqrt(r);
}

T3::Vector& T3::Vector::operator+=(T3::Vector& x) {
	CheckDims(O, x.O); CheckDims(N, x.N);
	FOR(a,x.N) T.at(a) += x(a);
	return *this;
}

T3::Vector::operator Scalar() {
	return T[0];
}

T3::Vector operator-(T3::Vector x) {
	T3::Vector y(x.O,x.N,(string)"-"+x.id);
	PFOR(i,x.N)
		y(i) = -x(i);
	return y;
}

T3::Vector operator+(T3::Vector x, T3::Vector y) {
	T3::Domain O = Merge(x.O,y.O); CheckDims(x.N, y.N);
	T3::Vector z(O,x.N,x.id+"+"+y.id);
	PFOR(i,x.N)
		z(i) = x(i) + y(i);
	return z;
}

T3::Vector operator-(T3::Vector x, T3::Vector y) {
	T3::Domain O = Merge(x.O,y.O); CheckDims(x.N, y.N);
	T3::Vector z(O,x.N,x.id+"-"+y.id);
	PFOR(i,x.N)
		z(i) = x(i) - y(i);
	return z;
}

T3::Scalar operator*(T3::Vector x, T3::Vector y) {
	T3::Domain O = Merge(x.O, y.O); CheckDims(x.N, y.N);
	T3::Scalar z(O);
	FOR(i,x.N)
		z = z + x(i)*y(i);
	z.id = x.id+"*"+y.id;
	return z;
}

T3::Vector operator%(T3::Vector x, T3::Vector y) {
	T3::Domain O = Merge(x.O, y.O); CheckDims(x.N, y.N); CheckDims(x.N, 3);
	T3::Vector z(O, x.N, x.id+"%"+y.id);
	z(0) = x(1)*y(2)-x(2)*y(1);
	z(1) = x(2)*y(0)-x(0)*y(2);
	z(2) = x(0)*y(1)-x(1)*y(0);
	return z;
}

T3::Vector operator*(T3::Vector x, T3::Scalar y) {
	T3::Domain O = Merge(x.O, y.O);
	T3::Vector z(O,x.N);
	PFOR(i,x.N)
		z(i) = x(i)*y;
	return z;
}

T3::Vector operator*(T3::Scalar x, T3::Vector y) {
	return y*x;
}

T3::Vector operator*(T3::Vector x, real y) {
	T3::Vector z(x.O,x.N);
	PFOR(i,x.N)
		z(i) = x(i)*y;
	return z;
}

T3::Vector operator*(real x, T3::Vector y) {
	return y*x;
}

T3::Vector operator/(T3::Vector x, T3::Scalar y) {
	T3::Domain O = Merge(x.O, y.O);
	T3::Vector z(O,x.N);
	PFOR(i,x.N)
		z(i) = x(i)/y;
	return z;
}

T3::Vector operator/(T3::Vector x, real y) {
	return x*(1./y);
}

T3::Vector diffOld(T3::Scalar x) {
	T3::Vector dx(x.O, x.O.N, (std::string)"d"+x.id);
	FOR(p,dx.N) {
		int dg = x.O.upto(p);
		int dc = x.O.upto(p-1);
		int G = x.O()/dg;
		int C = x.O(p);
		FOR(g,G) {
			FOR(m,dc) {
				int i = g*dg + m;
				dx(p)[i] = (x[i+dc]-x[i])/dx.O.d(p);
//				dx(p)[i] = (-3*x[i]+4*x[i+dc]-x[i+2*dc])/(2*dx.O.d(p));
				for ( int c=1; c<C-1; c++ ) {
					i = g*dg + m + c*dc;
					dx(p)[i] = (x[i+dc]-x[i-dc])/(2*dx.O.d(p));
				}
				i = g*dg + m + (C-1)*dc;
//				dx(p)[i] = (3*x[i]-4*x[i-dc]+x[i-2*dc])/(2*dx.O.d(p));
				dx(p)[i] = (x[i]-x[i-dc])/dx.O.d(p);
			}
		}
	}
	return dx;
}

#define D1 0.6666666666666666666666
#define D2 -0.083333333333333333333
#define D00 -1.4117647058823529411764705882353
#define D01 1.7352941176470588235294117647059
#define D02 -0.23529411764705882352941176470588
#define D03 -0.088235294117647058823529411764706
#define D10 -0.5
#define D12 0.5
#define D20 0.093023255813953488372093023255814
#define D21 -0.68604651162790697674418604651163
#define D23 0.68604651162790697674418604651163
#define D24 -0.093023255813953488372093023255814
#define D30 0.030612244897959183673469387755102
#define D32 -0.60204081632653061224489795918367
#define D34 0.65306122448979591836734693877551
#define D35 -0.081632653061224489795918367346939

T3::Vector diffNew(T3::Scalar x) {
	T3::Vector dx(x.O, x.O.N, (std::string)"d"+x.id);
	FOR(p,dx.N) {
		int dg = x.O.upto(p);
		int dc = x.O.upto(p-1);
		int G = x.O()/dg;
		int C = x.O(p);
		FOR(g,G) {
			FOR(m,dc) {
				int i = g*dg + m;
				dx(p)[i] = (D00*x[i]+D01*x[i+dc]+D02*x[i+2*dc]+D03*x[i+3*dc])/dx.O.d(p);
				dx(p)[i+dc] = (D10*x[i]+D12*x[i+2*dc])/dx.O.d(p);
				dx(p)[i+2*dc] = (D20*x[i]+D21*x[i+dc]+D23*x[i+3*dc]+D24*x[i+4*dc])/dx.O.d(p);
				dx(p)[i+3*dc] = (D30*x[i]+D32*x[i+2*dc]+D34*x[i+4*dc]+D35*x[i+5*dc])/dx.O.d(p);
				for ( int c=4; c<C-4; c++ ) {
					i = g*dg + m + c*dc;
					dx(p)[i] = (-D2*x[i-2*dc]-D1*x[i-dc]+D1*x[i+dc]+D2*x[i+2*dc])/dx.O.d(p);
				}
				i = g*dg + m + (C-1)*dc;
				dx(p)[i-3*dc] = -(D35*x[i-5*dc]+D34*x[i-4*dc]+D32*x[i-2*dc]+D30*x[i])/dx.O.d(p);
				dx(p)[i-2*dc] = -(D24*x[i-4*dc]+D23*x[i-3*dc]+D21*x[i-dc]+D20*x[i])/dx.O.d(p);
				dx(p)[i-dc] = -(D12*x[i-2*dc]+D10*x[i])/dx.O.d(p);
				dx(p)[i] = -(D03*x[i-3*dc]+D02*x[i-2*dc]+D01*x[i-dc]+D00*x[i])/dx.O.d(p);
			}
		}
	}
	return dx;
}

T3::Vector diffOlder(T3::Scalar x) {
	bool runNew = true;
	FOR(n,x.O.N) if ( x.O(n) < 8 ) { runNew = false; break; };
	if ( runNew ) return diffNew(x);
	else return diffOld(x);
	return diffNew(x);
}

void diffOne(T3::Scalar x, T3::Scalar& dx, int i, int dc, int C, real del) {
    dx[i] = 0;
}

void diffTwo(T3::Scalar x, T3::Scalar& dx, int i, int dc, int C, real del) {
    dx[i] = dx[i+dc] = (x[i+dc]-x[i])/del;
}

void diffThree(T3::Scalar x, T3::Scalar& dx, int i, int dc, int C, real del) {
    dx[i] = (-3*x[i]+4*x[i+dc]-x[i+2*dc])/(2*del);
    for ( int c=1; c<C-1; c++ )
        dx[i+c*dc] = (-x[i+(c-1)*dc]+x[i+(c+1)*dc])/(2*del);
    dx[i+(C-1)*dc] = (x[i+(C-3)*dc]-4*x[i+(C-2)*dc]+3*x[i+(C-1)*dc])/(2*del);
}

void diffSix(T3::Scalar x, T3::Scalar& dx, int i, int dc, int C, real del) {
	dx[i] = (D00*x[i]+D01*x[i+dc]+D02*x[i+2*dc]+D03*x[i+3*dc])/del;
	dx[i+dc] = (D10*x[i]+D12*x[i+2*dc])/del;
	dx[i+2*dc] = (D20*x[i]+D21*x[i+dc]+D23*x[i+3*dc]+D24*x[i+4*dc])/del;
	dx[i+3*dc] = (D30*x[i]+D32*x[i+2*dc]+D34*x[i+4*dc]+D35*x[i+5*dc])/del;
	for ( int c=4; c<C-4; c++ )
		dx[i+c*dc] = (-D2*x[i+(c-2)*dc]-D1*x[i+(c-1)*dc]+D1*x[i+(c+1)*dc]+D2*x[i+(c+2)*dc])/del;
	dx[i+(C-4)*dc] = -(D35*x[i+(C-6)*dc]+D34*x[i+(C-5)*dc]+D32*x[i+(C-3)*dc]+D30*x[i+(C-1)*dc])/del;
	dx[i+(C-3)*dc] = -(D24*x[i+(C-5)*dc]+D23*x[i+(C-4)*dc]+D21*x[i+(C-2)*dc]+D20*x[i+(C-1)*dc])/del;
	dx[i+(C-2)*dc] = -(D12*x[i+(C-3)*dc]+D10*x[i+(C-1)*dc])/del;
	dx[i+(C-1)*dc] = -(D03*x[i+(C-4)*dc]+D02*x[i+(C-3)*dc]+D01*x[i+(C-2)*dc]+D00*x[i+(C-1)*dc])/del;
}

T3::Vector diff(T3::Scalar x) {
	T3::Vector dx(x.O, x.O.N, (std::string)"d"+x.id);
	PFOR(p,dx.N) {
		int dg = x.O.upto(p);
		int dc = x.O.upto(p-1);
		int G = x.O()/dg;
		int C = x.O(p);
		FOR(g,G) {
			FOR(m,dc) {
				int i = g*dg + m;
				/*if ( C > 5 ) diffSix(x, dx(p), i, dc, C, dx.O.d(p));
				else*/ if ( C > 2 ) diffThree(x, dx(p), i, dc, C, dx.O.d(p));
				else if ( C > 1 ) diffTwo(x, dx(p), i, dc, C, dx.O.d(p));
				else diffOne(x, dx(p), i, dc, C, dx.O.d(p));
			}
		}
	}
	return dx;
}

T3::Scalar diss(T3::Scalar x) {
	T3::Vector dx(x.O, x.O.N, (std::string)"d4"+x.id);
	PFOR(p,dx.N) {
		int dg = x.O.upto(p);
		int dc = x.O.upto(p-1);
		int G = x.O()/dg;
		int C = x.O(p);
		FOR(g,G) {
			FOR(m,dc) {
				int i = g*dg + m;
				dx(p)[i] = (2*x[i]-4*x[i+dc]+2*x[i+2*dc])/dx.O.d(p);
				dx(p)[i+dc] = (-2*x[i]+5*x[i+dc]-4*x[i+2*dc]+x[i+3*dc])/dx.O.d(p);
				for ( int c=2; c<C-2; c++ ) {
					i = g*dg + m + c*dc;
					dx(p)[i] = (x[i-2*dc]-4*x[i-dc]+6*x[i]-4*x[i+dc]+x[i+2*dc])/dx.O.d(p);
				}
				i = g*dg + m + (C-1)*dc;
				dx(p)[i-dc] = (x[i-3*dc]-4*x[i-2*dc]+5*x[i-dc]-2*x[i])/dx.O.d(p);
				dx(p)[i] = (2*x[i-2*dc]-4*x[i-dc]+2*x[i])/dx.O.d(p);
			}
		}
	}
	T3::Scalar Dx(x.O, dx.id);
	FOR(p,dx.N) Dx = Dx + dx(p);
	return Dx;
}

T3::Vector diss(T3::Vector x) {
	T3::Vector dx(x.O, x.O.N, (std::string)"d4"+x.id);
	PFOR(p,dx.N) dx(p) = diss(x(p));
	return dx;
}

T3::Vector LinCom(real a, T3::Vector w, real b, T3::Vector x, std::string id) {
	CheckDims(w.N, x.N);
	T3::Domain O = Merge(w.O,x.O);
	T3::Vector v(O, w.N, id);
	PFOR(n,v.N) v(n) = LinCom(a,w(n),b,x(n));
//	FOR(n,v.N) FORO v(n)[o] = a*w(n)[o]+b*x(n)[o];
	return v;
}

T3::Vector LinCom(real a, T3::Vector w, real b, T3::Vector x, real c, T3::Vector y, std::string id) {
	CheckDims(w.N, x.N); CheckDims(x.N, y.N);
	T3::Domain O = Merge(Merge(w.O,x.O),y.O);
	T3::Vector v(O, w.N, id);
	PFOR(n,v.N) v(n) = LinCom(a,w(n),b,x(n),c,y(n));
//	FOR(n,v.N) FORO v(n)[o] = a*w(n)[o]+b*x(n)[o]+c*y(n)[o];
	return v;
}

T3::Vector LinCom(real a, T3::Vector w, real b, T3::Vector x, real c, T3::Vector y, real d, T3::Vector z, std::string id) {
	CheckDims(w.N, x.N); CheckDims(x.N, y.N); CheckDims(y.N, z.N);
	T3::Domain O = Merge(Merge(Merge(w.O,x.O),y.O),z.O);
	T3::Vector v(O, w.N, id);
	PFOR(n,v.N) v(n) = LinCom(a,w(n),b,x(n),c,y(n),d,z(n));
//	FOR(n,v.N) FORO v(n)[o] = a*w(n)[o]+b*x(n)[o]+c*y(n)[o]+d*z(n)[o];
	return v;
}

T3::Vector LinCom(real a, T3::Vector w, real b, T3::Vector x, real c, T3::Vector y, real d, T3::Vector z, real e, T3::Vector s, std::string id) {
	CheckDims(w.N, x.N); CheckDims(x.N, y.N); CheckDims(y.N, z.N); CheckDims(z.N, s.N);
	T3::Domain O = Merge(Merge(Merge(Merge(w.O,x.O),y.O),z.O),s.O);
	T3::Vector v(O, w.N, id);
	PFOR(n,v.N) v(n) = LinCom(a,w(n),b,x(n),c,y(n),d,z(n),e,s(n));
	//	FOR(n,v.N) FORO v(n)[o] = a*w(n)[o]+b*x(n)[o]+c*y(n)[o]+d*z(n)[o];
	return v;
}
